/*
 * control_step.cpp
 *
 *  Created on: 2020年9月2日
 *      Author: 17900
 */
#include "control_step.hpp"



CONTROL_STEP control_step;

void CONTROL_STEP::Control_Step()
{
	xQueuePeek(queueRCCommand, &rcCommand, 0);					//从队列中获取遥控器数据
	Control_Feedback();       //控制测量反馈
//	ControlRC_Check(CtrlIO);          //检查遥控器信号
	Integral_Reset();       //起降重置积分
	//--------------保存上次模式信息---------//

	CtrlIO.last_control_mode = CtrlIO.control_mode;
	CtrlIO.last_yaw_mode     = CtrlIO.yaw_mode    ;
	CtrlIO.last_fly_mode     = CtrlIO.fly_mode    ;

	//----------------摇杆信号---------------//

	CtrlIO.input[0] =  rcCommand.Val[0];
	CtrlIO.input[1] = -rcCommand.Val[1];
	CtrlIO.input[2] =  rcCommand.Val[2];
	CtrlIO.input[3] =  rcCommand.Val[3];

	//----------------飞行模式---------------//

	if(rcCommand.Key[3]==0 )
	{
		CtrlIO.fly_mode=0;//操控模式
	}
	else if (rcCommand.Key[3]==2 )
	{
		CtrlIO.fly_mode=1;//轨迹模式
		if (CtrlIO.last_fly_mode==0)
		{
//			Pos_Hold_Init(&trajectory);
		}

//		Trajectory_Target_Receive(&trajectory);
//		Trajectory_Step(&trajectory,trajectory.trajectory_mod);

	}

	//----------------锁尾模式---------------//

	if      (rcCommand.Key[2]==2)   CtrlIO.yaw_mode=0;//解除锁尾
	else if (rcCommand.Key[2]==0)   CtrlIO.yaw_mode=1;//锁尾，仅限直升机模式

	//----------------控制模式---------------//

	if (CtrlIO.rc_status!=LOST)
	{
		switch(rcCommand.Key[0])
		{
			case 0:
				CtrlIO.control_mode = 0;	//手控
				break;
			case 1:
				CtrlIO.control_mode=1;	//半自控
				break;
			case 2:
				if (CtrlIO.fly_mode==0) CtrlIO.control_mode = 2;			//速度操纵
				else                    CtrlIO.control_mode = 3;			//轨迹规划
				break;
			default:break;
		}
	}
	else
	{
		CtrlIO.control_mode = 4;	//失控保护
	}

	//----------------控制逻辑----------------//

	switch(CtrlIO.control_mode)
	{
		case 0://手控
		  In_Loop_Step();              //角度环(锁尾)
		  CtrlLpIO.pqr_command[0] = CtrlIO.input[0]*0.18f*D2R;//滚转角速度输入0.225对应90°/s
		  CtrlLpIO.pqr_command[1] = CtrlIO.input[1]*0.18f*D2R;//俯仰角速度输入0.225对应90°/s
		  CtrlLpIO.thrust_command[0] = (CtrlIO.input[3]-1500)/(498)*OneG + OneG;//拉力输入，遥控器1500pwm对应1g拉力
//		  Control_Output(CtrlIO);           //角速度环（PID）
		  break;
		case 1://半自控
		  CtrlLpIO.roll_command[0] = CtrlIO.input[0]*0.04f*D2R;//滚转角度输入0.05对应20°
		  CtrlLpIO.pitch_command[0] = CtrlIO.input[1]*0.04f*D2R;//俯仰角度输入0.05对应20°
		  CtrlLpIO.thrust_command[0] = (CtrlIO.input[3]-1500)/(498)*OneG + OneG;//拉力输入，遥控器1500pwm对应1g拉力
		  CtrlLpIO.AngleLimitR       = 20*D2R;//滚转角限幅
		  CtrlLpIO.AngleLimitP       = 20*D2R;//俯仰角限幅
		  In_Loop_Step();              //角度环
//		  Control_Output(CtrlIO);              //角速度环（PID）
		  break;
		case 2://速度操纵
		 // Out_Loop_XY_Pre();           //水平速度给定，判断定点模式
		  Out_Loop_Z_Pre();            //高度给定
			CtrlLpIO.X_command[1] = -CtrlIO.input[1]*0.0025f;//机头坐标横向速度输入0.0125对应5m/s
			CtrlLpIO.Y_command[1] =  CtrlIO.input[0]*0.0025f;//机头坐标纵向速度输入0.0125对应5m/s
          Out_Loop_Step();             //速度控制
		  Underactuated_Loop_Step();   //欠驱动环（欠驱动解算，拉力补偿）
		  CtrlLpIO.AngleLimitR       = 20*D2R;//滚转角限幅
		  CtrlLpIO.AngleLimitP       = 20*D2R;//俯仰角限幅
	      In_Loop_Step();              //角度环
//          Control_Output2(CtrlIO);             //角速度环（MRAC）
		  break;
		case 3://轨迹
//		  Pos_Loop_Step(CtrlLpIO);
          Out_Loop_Step();             //速度控制
		  Underactuated_Loop_Step();   //欠驱动环（欠驱动解算，拉力补偿）
		  CtrlLpIO.AngleLimitR       = 20*D2R;//滚转角限幅
		  CtrlLpIO.AngleLimitP       = 20*D2R;//俯仰角限幅
	      In_Loop_Step();              //角度环
//		  Control_Output2(CtrlIO);             //角速度环（MRAC）
		  break;
		case 4://失控保护
		  CtrlLpIO.roll_command[0] = 0;      //滚转角度输入0
		  CtrlLpIO.pitch_command[0] = 0;      //俯仰角度输入0
		  In_Loop_Step();              //角度环
//		  Control_Output(CtrlIO);              //角速度环（PID）
		  break;
		default:break;
	}

	//---------------------------------------//
}




void CONTROL_STEP::Control_Step2()
{
	switch(CtrlIO.control_mode)
	{
		case 0://手控
		  Control_Output();           //角速度环（PID）
		  break;
		case 1://半自控
		  Control_Output();              //角速度环（PID）
		  break;
		case 2://速度操纵
          Control_Output();             //角速度环（MRAC）
		  break;
		case 3://轨迹
		  Control_Output();             //角速度环（MRAC）
		  break;
		case 4://失控保护
		  Control_Output();              //角速度环（PID）
		  break;
		default:break;
	}
	Output_To_Motor();          //控制输出到执行器
	Integral_Restore();

}


void CONTROL_STEP::PID_Para_Update(void)
{
	xQueuePeek(queuePID, &pid_msg, 0);
	for(u8 i=0;i<18;i++)
		pid[i].Pid_Set_Para(pid_msg.kp[i], pid_msg.ki[i], pid_msg.kd[i]);
}


void CONTROL_STEP::Tranfer_Data_Update(void)
{
	control_data.Ang[0] = CtrlFbck.Ang[0];
	control_data.Ang[1] = CtrlFbck.Ang[1];
	control_data.Ang[2] = CtrlFbck.Ang[2];

	control_data.X[0] = CtrlFbck.X[0];
	control_data.X[1] = CtrlFbck.X[1];
	control_data.Y[0] = CtrlFbck.Y[0];
	control_data.Y[1] = CtrlFbck.Y[1];
	control_data.Z[0] = CtrlFbck.Z[0];
	control_data.Z[1] = CtrlFbck.Z[1];

	control_data.XH[0] = CtrlFbck.XH[0];
	control_data.XH[1] = CtrlFbck.XH[1];
	control_data.YH[0] = CtrlFbck.YH[0];
	control_data.YH[1] = CtrlFbck.YH[1];

	control_data.X_command[0] = CtrlLpIO.X_command[0];
	control_data.X_command[1] = CtrlLpIO.X_command[1];
	control_data.Y_command[0] = CtrlLpIO.Y_command[0];
	control_data.Y_command[1] = CtrlLpIO.Y_command[1];
	control_data.Z_command[0] = CtrlLpIO.Z_command[0];
	control_data.Z_command[1] = CtrlLpIO.Z_command[1];

	control_data.rc_status = CtrlIO.rc_status;
	control_data.brake_mode = CtrlLpIO.brake_mode;
	control_data.posion_mode = CtrlLpIO.position_mode;

	control_data.mt_output[0] = CtrlIO.mt_output[0];
	control_data.mt_output[1] = CtrlIO.mt_output[1];

	control_data.output[0] = CtrlIO.output[0];
	control_data.output[1] = CtrlIO.output[1];
	control_data.output[2] = CtrlIO.output[2];
	control_data.output[3] = CtrlIO.output[3];
	control_data.output[4] = CtrlIO.output[4];

	control_data.pitch_command[0] = CtrlLpIO.pitch_command[0];
	control_data.pitch_command[1] = CtrlLpIO.pitch_command[1];
	control_data.roll_command[0] = CtrlLpIO.roll_command[0];
	control_data.roll_command[1] = CtrlLpIO.roll_command[1];
	control_data.yaw_command[0] = CtrlLpIO.yaw_command[0];
	control_data.yaw_command[1] = CtrlLpIO.yaw_command[1];


	control_data.pqr[0] = CtrlFbck.pqr[0];
	control_data.pqr[1] = CtrlFbck.pqr[1];
	control_data.pqr[2] = CtrlFbck.pqr[2];
	control_data.pqr_command[0] = CtrlLpIO.pqr_command[0];
	control_data.pqr_command[1] = CtrlLpIO.pqr_command[1];
	control_data.pqr_command[2] = CtrlLpIO.pqr_command[2];

	xQueueOverwrite(queueControlTransfer,&control_data);
}

extern "C" void control_main(void *argument)
{
	static u32 cnt = 0;
	control_step.Control_Init();
	osDelay(10);
	for(;;)
	{
		osSemaphoreAcquire(semControl,0xffffffff);
		control_step.Control_Step();
		control_step.Control_Step2();
		if(cnt%200)
			control_step.PID_Para_Update();
		if(cnt%4)
			control_step.Tranfer_Data_Update();
		cnt++;
	}
}
